# coding=utf-8
# 卡尔曼滤波
from numpy import *
import matplotlib.pyplot as plt

# 字体设置
from matplotlib.font_manager import FontProperties

font_set = FontProperties(fname=r"c:\windows\fonts\simsun.ttc", size=12)


class KalmanFiltordata:
    kalman = {
        'LastBestP': 0.0,  # 上一次估计误差
        'lastout': 0.0,  # 最后输出值
        'lastKn': 0.0,  # 上次的卡尔曼增益
        'error': 0.0  # 测量误差
    }

    def __init__(self, lastout, LastBestP=5, lastKn=0, error=1):
        self.kalman['LastBestP'] = LastBestP
        self.kalman['lastout'] = lastout
        self.kalman['lastKn'] = lastKn
        self.kalman['error'] = error

    # 一维度卡尔曼滤波
    def KalmanFiltor(self, input):
        # 卡尔曼增益
        kn = self.kalman['LastBestP'] / (self.kalman['LastBestP'] + self.kalman['error'])
        # 估计最优值
        bestOUT = self.kalman['lastout'] + kn * (input - self.kalman['lastout'])
        # 更新估计误差
        bastP = (1 - kn) * self.kalman['LastBestP']  # 有问题
        # 更新记录
        self.kalman['lastKn'] = kn
        self.kalman['lastout'] = bestOUT
        self.kalman['LastBestP'] = bastP
        return bestOUT


class KalmanFiltordata2:
    kalman = {
        'LastP': 0.0,
        'Now_P': 0.0,
        'out': 0.0,
        'Kg': 0.0,
        'Q': 0.0,
        'R': 0.0,
    }

    def __init__(self, Q=0.001, R=0.543):
        self.kalman['LastP'] = 0.02
        self.kalman['Now_P'] = 0
        self.kalman['out'] = 0
        self.kalman['Kg'] = 0
        self.kalman['Q'] = Q
        self.kalman['R'] = R

        # 卡尔曼滤波

    def KalmanFiltor(self, input: float):
        self.kalman['Now_P'] = self.kalman['LastP'] + self.kalman['Q']
        self.kalman['Kg'] = self.kalman['Now_P'] / (self.kalman['Now_P'] + self.kalman['R'])
        self.kalman['out'] = self.kalman['out'] + self.kalman['Kg'] * (input - self.kalman['out'])
        self.kalman['LastP'] = (1 - self.kalman['Kg']) * self.kalman['Now_P']
        return self.kalman['out']


def show():
    for k in arange(0, 0.5, 0.001):
        # 范围
        times = 15
        # x的精度
        step = 0.1
        # 噪声范围
        NoiseArea = 20
        kalman = KalmanFiltordata2(0.02, 0.543)
        x = arange(0, times, step)
        Filtoreddata = []  # 过滤后数据
        Primitivedata = []  # 原始测量数据
        y = []
        for i in arange(0, times / 2, step):
            tmp = 20 * sin(i) + random.randint(-NoiseArea, NoiseArea)  # 添加噪声
            out = kalman.KalmanFiltor(tmp)
            Filtoreddata.append(out)
            Primitivedata.append(tmp)
            y.append(20 * sin(i))

        for i in arange(0, times / 2, step):
            tmp = 20 * i + random.randint(-NoiseArea, NoiseArea)  # 添加噪声
            out = kalman.KalmanFiltor(tmp)
            Filtoreddata.append(out)
            Primitivedata.append(tmp)
            y.append(20 * i)

        plt.plot(x, y, label="理论数据")
        plt.plot(x, Filtoreddata, label="过滤后的数据")
        plt.plot(x, Primitivedata, label="测量数据")

        plt.legend(prop=font_set)
        plt.title(k)
        plt.show(block=False)

        plt.pause(0.1)
        plt.clf()


show()
